#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <unordered_map>
#include <unordered_set>
#include <algorithm>

using namespace std;

struct Pos
{
    int16_t x_;
    int16_t y_;

    Pos() : x_(-1), y_(-1) {}
    Pos(int16_t x, int16_t y) : x_(x), y_(y){};
};
ostream &operator<<(ostream &out, const Pos &p)
{
    cout << " (" << p.x_ << "," << p.y_ << ") ";
    return out;
}
bool operator==(const Pos &lhs, const Pos &rhs)
{
    return lhs.x_ == rhs.x_ && lhs.y_ == rhs.y_;
}
struct PosHash
{
    size_t operator()(const Pos &hs) const
    {
        return hs.x_ ^ (hs.y_ << 2);
    }
};
struct PosDirectComp
{
    bool operator()(const Pos &lhs, const Pos &rhs)
    {
        int dx = end.x_ - start.x_;
        int dy = end.y_ - start.y_;
        if (dx)
            dx = dx > 0 ? 1 : -1;
        if (dy)
            dy = dy > 0 ? 1 : -1;
        int l = abs(lhs.x_ - dx) + abs(lhs.y_ - dy);
        int r = abs(rhs.x_ - dx) + abs(rhs.y_ - dy);
        return l < r;
    }
    Pos start;
    Pos end;
};
struct Pre
{
    int16_t x_;
    int16_t y_;
    int32_t cost_;
};
struct State
{
    int16_t x_;
    int16_t y_;
    int32_t cost_;  //从起点的消耗值
    int32_t weigh_; // cost_ + 到终点预估值
    int8_t directX_;
    int8_t directY_;
    bool operator<(const State &s) const
    {
        return this->weigh_ > s.weigh_;
    }
};
ostream &operator<<(ostream &out, const State &s)
{
    out << "  " << s.x_ << " " << s.y_ << "| " << (int)s.directX_ << " " << (int)s.directY_ << "| " << s.cost_ << " " << s.weigh_;
    return out;
}

static const int allow = 0;
static const int forbid = 1;
static const int weighl = 100;
static const int weighh = 141;
static const vector<Pos> directs = {{0, 1}, {0, -1}, {1, 0}, {-1, 0}, {1, 1}, {-1, 1}, {1, -1}, {-1, -1}};
static const vector<Pre> directCosts = {
    {0, 1, weighl}, {0, -1, weighl}, {1, 0, weighl}, {-1, 0, weighl}, {1, 1, weighh}, {-1, 1, weighh}, {1, -1, weighh}, {-1, -1, weighh}};

struct Jps
{
    vector<vector<int>> mapInfo_;
    int mapLength() { return mapInfo_.size(); }
    int mapWidth() { mapInfo_.size() ? 0 : mapInfo_[0].size(); }

    void jps(Pos start, Pos end, vector<Pos> &result);
    bool forceNeighbor(State start, Pos end, Pos direct, priority_queue<State> &openList, unordered_map<Pos, Pos, PosHash> &pres);
    bool jumpPoint(State start, Pos end, Pos direct, priority_queue<State> &openList, unordered_map<Pos, Pos, PosHash> &pres);

    void astar(Pos start, Pos end, vector<Pos> &result);

    bool walkAble(int x, int y);
    bool sideCheck(int x, int y);
    static int evaluateCost(Pos s, Pos e);
    static bool posEqual(Pos a, Pos b);
};

bool Jps::posEqual(Pos a, Pos b)
{
    if (a.x_ == b.x_ && a.y_ == b.y_)
        return true;
    return false;
}

bool Jps::sideCheck(int x, int y)
{
    if (x < 0 || x >= mapLength() || y < 0 || y >= mapWidth())
        return false;
    return true;
}

bool Jps::walkAble(int x, int y)
{
    if (x < 0 || x >= mapLength() || y < 0 || y >= mapWidth())
        return false;
    if (this->mapInfo_[x][y])
        return false;
    return true;
}

bool Jps::forceNeighbor(State start, Pos end, Pos direct, priority_queue<State> &openList, unordered_map<Pos, Pos, PosHash> &pres)
{
    bool b = false;

    int x1 = start.x_ + direct.y_;
    int y1 = start.y_ + direct.x_;
    int x11 = x1 + direct.x_;
    int y11 = y1 + direct.y_;
    if (this->sideCheck(x1, y1) && this->sideCheck(x11, y11))
    {
        if (!walkAble(x1, y1) && walkAble(x11, y11))
        {
            b = b || true;
            int cost = start.cost_ + weighh;
            int weigh = this->posEqual({x11, y11}, end) ? 0 : cost + this->evaluateCost({x11, y11}, end);
            State s{x11, y11, cost, weigh, direct.x_ + direct.y_, direct.y_ + direct.x_};
            if (pres.end() == pres.find({s.x_, s.y_}))
            {
                openList.push(s);
                cout << "jumpPoint1" << s << endl;
                pres[{s.x_, s.y_}] = {start.x_, start.y_};
            }
        }
    }

    int x2 = start.x_ - direct.y_;
    int y2 = start.y_ - direct.x_;
    int x21 = x2 + direct.x_;
    int y21 = y2 + direct.y_;
    if (this->sideCheck(x2, y2) && this->sideCheck(x21, y21))
    {
        if (!walkAble(x2, y2) && walkAble(x21, y21))
        {
            b = b || true;
            int cost = start.cost_ + weighh;
            int weigh = this->posEqual({x21, y21}, end) ? 0 : cost + this->evaluateCost({x21, y21}, end);
            State s{x21, y21, cost, weigh, direct.x_ - direct.y_, direct.y_ - direct.x_};
            if (pres.end() == pres.find({s.x_, s.y_}))
            {
                openList.push(s);
                cout << "jumpPoint2" << s << endl;
                pres[{s.x_, s.y_}] = {start.x_, start.y_};
            }
        }
    }
    return b;
}

bool Jps::jumpPoint(State start, Pos end, Pos direct, priority_queue<State> &openList, unordered_map<Pos, Pos, PosHash> &pres)
{
    State s = start;
    s.directX_ = direct.x_;
    s.directY_ = direct.y_;
    while (1)
    {
        if (!direct.x_ || !direct.y_)
        {
            s.x_ += direct.x_;
            s.y_ += direct.y_;
            s.cost_ += weighl;
            if (!this->walkAble(s.x_, s.y_))
                return false;
            if (this->posEqual({s.x_, s.y_}, end) || this->forceNeighbor(s, end, direct, openList, pres))
            {
                if (this->posEqual({s.x_, s.y_}, end))
                {
                    s.weigh_ = 0;
                }
                else
                {
                    s.weigh_ = s.cost_ + this->evaluateCost({s.x_, s.y_}, end);
                }
                if (pres.end() == pres.find({s.x_, s.y_}))
                {
                    openList.push(s);
                    cout << "jumpPoint3" << s << endl;
                    pres[{s.x_, s.y_}] = {start.x_, start.y_};
                    return true;
                }
            }
        }
        else
        {
            bool b = false;
            b = this->forceNeighbor(s, end, {direct.x_, 0}, openList, pres) || b;
            b = this->forceNeighbor(s, end, {0, direct.y_}, openList, pres) || b;
            if (b)
            {
                if (!this->posEqual({s.x_, s.y_}, {start.x_, start.y_}) && pres.end() == pres.find({s.x_, s.y_}))
                {
                    pres[{s.x_, s.y_}] = {start.x_, start.y_};
                }
                int x = s.x_;
                int y = s.y_;
                int dx = direct.x_;
                int dy = direct.y_;
                bool b1 = this->sideCheck(x + dx, y) && !walkAble(x + dx, y);
                bool b2 = this->sideCheck(x, y + dy) && !walkAble(x, y + dy);
                bool b3 = this->sideCheck(x + dx, y + dy) && walkAble(x + dx, y + dy);
                if ((b1 || b2) && b3)
                    return true;
            }

            s.x_ += direct.x_;
            s.y_ += direct.y_;
            s.cost_ += weighh;
            if (!this->walkAble(s.x_, s.y_))
                return false;
            if (this->posEqual({s.x_, s.y_}, end))
            {
                s.weigh_ = 0;
                if (pres.end() == pres.find({s.x_, s.y_}))
                {
                    openList.push(s);
                    cout << "jumpPoint4" << s << endl;
                    pres[{s.x_, s.y_}] = {start.x_, start.y_};
                }
            }
            bool r = false;
            r = this->jumpPoint(s, end, {direct.x_, 0}, openList, pres) || r;
            r = this->jumpPoint(s, end, {0, direct.y_}, openList, pres) || r;
            if (r)
            {
                if (pres.end() == pres.find({s.x_, s.y_}))
                {
                    pres[{s.x_, s.y_}] = {start.x_, start.y_};
                    s.x_ += direct.x_;
                    s.y_ += direct.y_;
                    s.cost_ += weighh;
                    s.weigh_ = this->posEqual({s.x_, s.y_}, end) ? 0 : s.cost_ + evaluateCost({s.x_, s.y_}, end);
                    if (walkAble(s.x_, s.y_) && pres.end() == pres.find({s.x_, s.y_}))
                    {
                        openList.push(s);
                        cout << "jumpPoint5" << s << endl;
                        pres[{s.x_, s.y_}] = {start.x_, start.y_};
                    }
                    return true;
                }
            }
        }
    }
}

int Jps::evaluateCost(Pos s, Pos e)
{
    double dis = pow((s.x_ - e.x_), 2) + pow((s.y_ - e.y_), 2);
    dis = sqrt(dis);
    return dis * 100;
}

void Jps::jps(Pos start, Pos end, vector<Pos> &result)
{
    if (!walkAble(start.x_, start.y_) || !walkAble(end.x_, end.y_))
        return;

    unordered_set<Pos, PosHash> closeList;
    priority_queue<State> openList;
    int weigh = this->evaluateCost(start, end);
    State stateS{start.x_, start.y_, 0, weigh, 0, 0};
    unordered_map<Pos, Pos, PosHash> pres;
    openList.push(stateS);

    while (!openList.empty())
    {
        State t = openList.top();
        openList.pop();
        cout << "Pop       " << t << endl;
        if (closeList.end() != closeList.find({t.x_, t.y_}))
            continue;
        closeList.insert({t.x_, t.y_});
        if (this->posEqual({t.x_, t.y_}, end))
        {
            result.push_back(end);
            Pos p(end.x_, end.y_);
            while (pres.end() != pres.find(p))
            {
                Pos pre = pres[p];
                p.x_ = pre.x_;
                p.y_ = pre.y_;
                result.push_back(p);
            }
            return;
        }

        int directX = t.directX_;
        int directY = t.directY_;
        vector<Pos> dps;
        if (!directX && !directY)
        {
            PosDirectComp p;
            p.start = start;
            p.end = end;
            dps = directs;
            sort(dps.begin(), dps.end(), p);
        }
        else if (directX && directY)
        {
            dps.push_back({directX, 0});
            dps.push_back({0, directY});
            dps.push_back({directX, directY});
        }
        else
        {
            dps.push_back({directX, directY});
        }
        for (int i = 0; i < dps.size(); ++i)
        {
            this->jumpPoint(t, end, dps[i], openList, pres);
        }
    }
}

void Jps::astar(Pos start, Pos end, vector<Pos> &result)
{
    if (!walkAble(start.x_, start.y_) && !walkAble(end.x_, end.y_))
        return;
    unordered_set<Pos, PosHash> closeList;
    unordered_map<Pos, Pre, PosHash> pres;
    priority_queue<State> openList;
    int weigh = this->evaluateCost(start, end);
    State stateS{start.x_, start.y_, 0, weigh};
    openList.push(stateS);

    while (!openList.empty())
    {
        State t = openList.top();
        openList.pop();
        cout << "atar pop" << t << endl;
        if (this->posEqual({t.x_, t.y_}, end))
        {
            result.push_back(end);
            Pos p(end.x_, end.y_);
            while (pres.end() != pres.find(p))
            {
                Pre pre = pres[p];
                p.x_ = pre.x_;
                p.y_ = pre.y_;
                result.push_back(p);
            }
            return;
        }
        closeList.insert({t.x_, t.y_});
        for (const auto &d : directCosts)
        {
            State t1{t.x_ + d.x_, t.y_ + d.y_, t.cost_ + d.cost_};
            if (!walkAble(t1.x_, t1.y_))
                continue;
            if (closeList.end() != closeList.find({t1.x_, t1.y_}))
                continue;

            t1.weigh_ = posEqual({t1.x_, t1.y_}, end) ? 0 : t1.cost_ + evaluateCost({t1.x_, t1.y_}, end);
            auto it = pres.find({t1.x_, t1.y_});
            if (it == pres.end() || it->second.cost_ > t1.cost_)
            {
                openList.push(t1);
                cout << "astar push" << t1 << endl;
                pres[{t1.x_, t1.y_}] = {t.x_, t.y_, t1.cost_};
            }
        }
    }
}

int main()
{
    Jps *jps = new Jps();
    jps->mapInfo_ = {
        {0, 0, 1, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 1, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 1, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 1, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 1, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 1, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
    };
    Pos start({0, 0});
    Pos end({7, 9});
    vector<Pos> result;
    // jps->jps(start, end, result);
    // jps->astar(start, end, result);
    for (auto &e : result)
    {
        jps->mapInfo_[e.x_][e.y_] = 5;
        cout << e;
    }
    cout << endl;
    for (int i = 0; i < jps->mapLength(); ++i)
    {
        for (int j = 0; j < jps->mapWidth(); ++j)
        {
            cout << jps->mapInfo_[i][j] << " ";
        }
        cout << endl;
    }
    delete jps;
}